/*
 *  main.cpp
 *  Preview3D
 *
 *  Created by Alec Jacobson on 11/15/10.
 *  Copyright 2010 New York University. All rights reserved.
 *
 */

#include "main.h"


#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <limits>

#if defined(_MACOSX)
#   include <GLUT/glut.h>
#else
#   include <GL/glut.h>
#endif

// Routine to set a quaternion from a rotation axis and angle
// ( input axis = float[3] angle = float  output: quat = float[4] )
void SetQuaternionFromAxisAngle(const float *axis, float angle, float *quat)
{
  float sina2, norm;
  sina2 = (float)sin(0.5f * angle);
  norm = (float)sqrt(axis[0]*axis[0] + axis[1]*axis[1] + axis[2]*axis[2]);
  quat[0] = sina2 * axis[0] / norm;
  quat[1] = sina2 * axis[1] / norm;
  quat[2] = sina2 * axis[2] / norm;
  quat[3] = (float)cos(0.5f * angle);
  
}


// Routine to convert a quaternion to a 4x4 matrix
// ( input: quat = float[4]  output: mat = float[4*4] )
void ConvertQuaternionToMatrix(const float *quat, float *mat)
{
  float yy2 = 2.0f * quat[1] * quat[1];
  float xy2 = 2.0f * quat[0] * quat[1];
  float xz2 = 2.0f * quat[0] * quat[2];
  float yz2 = 2.0f * quat[1] * quat[2];
  float zz2 = 2.0f * quat[2] * quat[2];
  float wz2 = 2.0f * quat[3] * quat[2];
  float wy2 = 2.0f * quat[3] * quat[1];
  float wx2 = 2.0f * quat[3] * quat[0];
  float xx2 = 2.0f * quat[0] * quat[0];
  mat[0*4+0] = - yy2 - zz2 + 1.0f;
  mat[0*4+1] = xy2 + wz2;
  mat[0*4+2] = xz2 - wy2;
  mat[0*4+3] = 0;
  mat[1*4+0] = xy2 - wz2;
  mat[1*4+1] = - xx2 - zz2 + 1.0f;
  mat[1*4+2] = yz2 + wx2;
  mat[1*4+3] = 0;
  mat[2*4+0] = xz2 + wy2;
  mat[2*4+1] = yz2 - wx2;
  mat[2*4+2] = - xx2 - yy2 + 1.0f;
  mat[2*4+3] = 0;
  mat[3*4+0] = mat[3*4+1] = mat[3*4+2] = 0;
  mat[3*4+3] = 1;
}


// Routine to multiply 2 quaternions (ie, compose rotations)
// ( input q1 = float[4] q2 = float[4]  output: qout = float[4] )
void MultiplyQuaternions(const float *q1, const float *q2, float *qout)
{
  float qr[4];
	qr[0] = q1[3]*q2[0] + q1[0]*q2[3] + q1[1]*q2[2] - q1[2]*q2[1];
	qr[1] = q1[3]*q2[1] + q1[1]*q2[3] + q1[2]*q2[0] - q1[0]*q2[2];
	qr[2] = q1[3]*q2[2] + q1[2]*q2[3] + q1[0]*q2[1] - q1[1]*q2[0];
	qr[3]  = q1[3]*q2[3] - (q1[0]*q2[0] + q1[1]*q2[1] + q1[2]*q2[2]);
  qout[0] = qr[0]; qout[1] = qr[1]; qout[2] = qr[2]; qout[3] = qr[3];
}

//  Callback function called when the 'AutoRotate' variable value of the tweak bar has changed
void TW_CALL SetAutoRotateCB(const void *value, void *clientData)
{
  (void)clientData; // unused
  
  g_AutoRotate = *(const int *)(value); // copy value to g_AutoRotate
  if( g_AutoRotate!=0 ) 
  {
    // init rotation
    g_RotateTime = glutGet(GLUT_ELAPSED_TIME);
    g_RotateStart[0] = g_Rotation[0];
    g_RotateStart[1] = g_Rotation[1];
    g_RotateStart[2] = g_Rotation[2];
    g_RotateStart[3] = g_Rotation[3];
    
    // make Rotation variable read-only
    TwDefine(" TweakBar/ObjRotation readonly ");
  }
  else
    // make Rotation variable read-write
    TwDefine(" TweakBar/ObjRotation readwrite ");
}

//  Callback function called by the tweak bar to get the 'AutoRotate' value
void TW_CALL GetAutoRotateCB(void *value, void *clientData)
{
  (void)clientData; // unused
  *(int *)(value) = g_AutoRotate; // copy g_AutoRotate to value
}


// Callback function called by GLUT to render screen
void Display(void)
{
  float v[4]; // will be used to set light paramters
  
  // Clear frame buffer
  glClearColor(
      background_color[0],
      background_color[1],
      background_color[2],
      background_color[3]);
  glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
  
  glEnable(GL_DEPTH_TEST);
  glDisable(GL_CULL_FACE);
  glEnable(GL_NORMALIZE);
  
  // Set light
  glEnable(GL_LIGHTING);
  glEnable(GL_LIGHT0);
  v[0] = v[1] = v[2] = g_LightMultiplier*0.4f; v[3] = 1.0f;
  glLightfv(GL_LIGHT0, GL_AMBIENT, v);
  v[0] = v[1] = v[2] = g_LightMultiplier*0.8f; v[3] = 1.0f;
  glLightfv(GL_LIGHT0, GL_DIFFUSE, v);
  v[0] = -g_LightDirection[0]; v[1] = -g_LightDirection[1]; v[2] = -g_LightDirection[2]; v[3] = 0.0f;
  glLightfv(GL_LIGHT0, GL_POSITION, v);
  
  // Set material
  glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT, g_MatAmbient);
  glMaterialfv(GL_FRONT_AND_BACK, GL_DIFFUSE, g_MatDiffuse);
  glMaterialfv(GL_FRONT_AND_BACK, GL_SPECULAR, g_MatSpecular);
  glMaterialf(GL_FRONT_AND_BACK, GL_SHININESS,g_MatShininess);
  
  push_scene();
  draw_mesh(
    vertices,
    faces,
    show_lines,
    line_color,
    show_faces,
    flat_shading);
  pop_scene();
  
  // Draw tweak bars
  TwDraw();
  
  // Present frame buffer
  glutSwapBuffers();
  
  // Recall Display at next frame
  glutPostRedisplay();
}


// Callback function called by GLUT when window size changes
void Reshape(int width, int height)
{
  // Set OpenGL viewport and camera
  glViewport(0, 0, width, height);
  glMatrixMode(GL_PROJECTION);
  glLoadIdentity();
  gluPerspective(40, (double)width/height, 1, 10);
  glMatrixMode(GL_MODELVIEW);
  glLoadIdentity();
  gluLookAt(0,0,5, 0,0,0, 0,1,0);
  // shift everything a little to the right
  glTranslatef(0.5f, 0.0f, 0.0f);
  
  // Send the new window size to AntTweakBar
  TwWindowSize(width, height);
}


// Function called at exit
void Terminate(void)
{ 
  TwTerminate();
}

void push_scene(){
    float mat[4*4]; // rotation matrix
    // Rotate scene
    glPushMatrix();
    // not sure why this was here
    //glTranslatef(0.5f, -0.3f, 0.0f);
    if( g_AutoRotate ) 
    {
        float axis[3] = { 0, 1, 0 };
        float angle = (float)(glutGet(GLUT_ELAPSED_TIME)-
            g_RotateTime)/1000.0f;
        float quat[4];
        SetQuaternionFromAxisAngle(axis, angle, quat);
        MultiplyQuaternions(g_RotateStart, quat, g_Rotation);
    }
    ConvertQuaternionToMatrix(g_Rotation, mat);
    glMultMatrixf(mat);
    glScaled(g_Zoom, g_Zoom, g_Zoom);
    glScaled(zoom, zoom, zoom);
    glTranslatef(shift[0],shift[1],shift[2]);
}

void pop_scene(){
    glPopMatrix();
}

void draw_mesh(
  const std::vector<std::vector<double> > & vertices,
  const std::vector<std::vector<size_t> > & faces,
  bool show_lines,
  float line_color[4],
  bool show_faces,
  bool flat_shading)
{
  GLuint faces_display_list = glGenLists(1);


  glNewList(faces_display_list, GL_COMPILE);
  // loop over all faces
  int face_index = 0;
  for(std::vector<std::vector<size_t> >::const_iterator fit = faces.begin();
      fit != faces.end();
      fit++,face_index++)
  {
    glBegin(GL_POLYGON);
    // flat shading
    if(flat_shading)
    {
      glNormal3f(
        face_normals[face_index][0],
        face_normals[face_index][1],
        face_normals[face_index][2]
        );
    }
    // loop over vertices in this face
    for(std::vector<size_t>::const_iterator vit = (*fit).begin();
        vit != (*fit).end();
        vit++)
    {
      // not flat shading goes here
      if(!flat_shading)
      {
        glNormal3f(
          vertex_normals[*vit][0],
          vertex_normals[*vit][1],
          vertex_normals[*vit][2]
          );
      }
      // assumes every point in vertices is length == 3
      glVertex3f(
        vertices[(*vit)][0],
        vertices[(*vit)][1],
        vertices[(*vit)][2]);
    }
    glEnd();
  }
  glEndList();

  glEnable(GL_LIGHTING);
  if(show_faces)
  {
    glEnable(GL_BLEND);
    glEnable(GL_POLYGON_OFFSET_FILL); // Avoid Stitching!
    glPolygonOffset(1.0, 1.0);
    //draw mesh faces
    glCallList(faces_display_list);
    glDisable(GL_POLYGON_OFFSET_FILL);
  }

  if(show_lines)
  {
    if(show_faces)
    {
      glDisable(GL_LIGHTING);
      glColor3fv(line_color);
    }
    glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
    glCallList(faces_display_list);
    glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
  }

  glDeleteLists(faces_display_list, 1);
}

void compute_face_normals(
  const std::vector<std::vector<double> > & vertices, 
  const std::vector<std::vector<size_t> > & faces,
  std::vector<std::vector<double> > & face_normals)
{
  face_normals.resize(faces.size());
  int face_index = 0;
  // loop over faces
  for(std::vector<std::vector<size_t> >::const_iterator fit = faces.begin();
      fit != faces.end();
      fit++,face_index++)
  {
    // Only valid for triangle meshes !!!
    std::vector<double> v1;
    v1.resize(3);
    v1[0] = vertices[(*fit)[1]][0] - vertices[(*fit)[0]][0];
    v1[1] = vertices[(*fit)[1]][1] - vertices[(*fit)[0]][1];
    v1[2] = vertices[(*fit)[1]][2] - vertices[(*fit)[0]][2];
    std::vector<double> v2;
    v2.resize(3);
    v2[0] = vertices[(*fit)[2]][0] - vertices[(*fit)[0]][0];
    v2[1] = vertices[(*fit)[2]][1] - vertices[(*fit)[0]][1];
    v2[2] = vertices[(*fit)[2]][2] - vertices[(*fit)[0]][2];
    std::vector<double> v1_cross_v2;
    v1_cross_v2.resize(3);
    v1_cross_v2[0] = v1[1]*v2[2] - v1[2]*v2[1];
    v1_cross_v2[1] = -(v1[0]*v2[2] - v1[2]*v2[0]);
    v1_cross_v2[2] = v1[0]*v2[1] - v1[1]*v2[0];
    double length = sqrt(
      v1_cross_v2[0]*v1_cross_v2[0] +
      v1_cross_v2[1]*v1_cross_v2[1] +
      v1_cross_v2[2]*v1_cross_v2[2]);
    v1_cross_v2[0] /= length;
    v1_cross_v2[1] /=  length;
    v1_cross_v2[2] /=  length;
    face_normals[face_index] = v1_cross_v2;
  }
}

void compute_vertex_normals(
  const std::vector<std::vector<double> > & vertices, 
  const std::vector<std::vector<size_t> > & faces,
  std::vector<std::vector<double> > & vertex_normals)
{  
  std::vector<std::vector<double> > face_normals;
  compute_face_normals(
    vertices,
    faces,
    face_normals);
  vertex_normals.resize(vertices.size());
  for(std::vector<std::vector<double> >::iterator vnit = vertex_normals.begin();
      vnit != vertex_normals.end();
      vnit++)
  {
    std::vector<double> normal;
    normal.resize(3);
    (*vnit) = normal;
  }

  int face_index = 0;
  // loop over faces
  for(std::vector<std::vector<size_t> >::const_iterator fit = faces.begin();
      fit != faces.end();
      fit++,face_index++)
  {
    // loop over vertices in this face
    for(std::vector<size_t>::const_iterator vit = (*fit).begin();
        vit != (*fit).end();
        vit++)
    {
      vertex_normals[*vit][0] += face_normals[face_index][0];
      vertex_normals[*vit][1] += face_normals[face_index][1];
      vertex_normals[*vit][2] += face_normals[face_index][2];
    }
  }

  // normalize
  for(int i=0; i<vertex_normals.size(); i++)
  {
    double length = sqrt(
      vertex_normals[i][0]*vertex_normals[i][0] +
      vertex_normals[i][1]*vertex_normals[i][1] +
      vertex_normals[i][2]*vertex_normals[i][2]);
    vertex_normals[i][0] /= length;
    vertex_normals[i][1] /= length;
    vertex_normals[i][2] /= length;
  }
}

bool read_obj(
  char* obj_file_name,
  std::vector<std::vector<double> > & vertices, 
  std::vector<std::vector<size_t> > & faces)
{
  FILE * obj_file = fopen(obj_file_name,"r");                                       
  if(NULL==obj_file)
  {
    printf("IOError: %s could not be opened...",obj_file_name);
    return false; 
  }

  vertices.clear();
  faces.clear();
  char type;
  while(fscanf(obj_file, "%c",&type)==1)
  {
    if(type == 'v')
    {
      float x,y,z;
      fscanf(obj_file, "%g %g %g\n",&x,&y,&z);
      std::vector<double> vertex;
      vertex.resize(3);
      vertex[0] = x;
      vertex[1] = y;
      vertex[2] = z;
      vertices.push_back(vertex);
    }else if(type == 'f')
    {
      std::vector<size_t> face;
      size_t i;
      int n;
      while(fscanf(obj_file,"%u",&i)==1)
      {
        face.push_back(i-1);
      }
      faces.push_back(face);
    }else if(type == '#')
    {
      //ignore comments
      char comment[1000];
      fscanf(obj_file,"%[^\n]",comment);
    }
  }
  printf("%d face%s\n",faces.size(),(faces.size() == 1 ? "" : "s"));
  printf("%d vert%s\n",vertices.size(),(vertices.size() == 1 ? "ex" : "ices"));
  number_of_vertices = vertices.size();
  number_of_faces = faces.size();
}

void get_scale_and_shift_to_fit_mesh( 
  const std::vector<std::vector<double> > & vertices, 
  const std::vector<std::vector<size_t> > & faces,
  float& zoom,
  float shift[])
{
  //Compute mesh centroid
  std::vector<double> centroid;
  centroid.resize(3);
  std::vector<double> min_point;
  min_point.resize(3,std::numeric_limits<double>::max());
  std::vector<double> max_point;
  max_point.resize(3,-std::numeric_limits<double>::max());
  for(std::vector<std::vector<double> >::const_iterator vit = vertices.begin();
      vit != vertices.end();
      vit++)
  {
    for(int i=0;i<3;i++)
    {
      centroid[i] += (*vit)[i];
      if(min_point[i] > (*vit)[i])
      {
        min_point[i] = (*vit)[i];
      }
      if(max_point[i] < (*vit)[i])
      {
        max_point[i] = (*vit)[i];
      }
    }
  }
  centroid[0] /= vertices.size();
  centroid[1] /= vertices.size();
  centroid[2] /= vertices.size();

  shift[0] = -centroid[0];
  shift[1] = -centroid[1];
  shift[2] = -centroid[2];
  double x_scale = fabs(max_point[0] - min_point[0]);
  double y_scale = fabs(max_point[1] - min_point[1]);
  double z_scale = fabs(max_point[2] - min_point[2]);
  zoom = 2.0/ std::max(z_scale,std::max(x_scale,y_scale));
}

// Main
int main(int argc, char *argv[])
{
  TwBar *bar; // Pointer to the tweak bar
  float axis[] = { 0.7f, 0.7f, 0.0f }; // initial model rotation
  float angle = 0.8f;
  
  // Initialize AntTweakBar
  // (note that AntTweakBar could also be intialized after GLUT, no matter)
  if( !TwInit(TW_OPENGL, NULL) )
  {
    // A fatal error occured    
    fprintf(stderr, "AntTweakBar initialization failed: %s\n", TwGetLastError());
    return 1;
  }

  // Initialize GLUT
  glutInit(&argc, argv);
  glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGBA | GLUT_DEPTH);
  glutInitWindowSize(800, 600);
  // Center window
  glutInitWindowPosition(
      glutGet(GLUT_SCREEN_WIDTH)/2-glutGet(GLUT_INIT_WINDOW_WIDTH)/2,
      glutGet(GLUT_SCREEN_HEIGHT)/2-glutGet(GLUT_INIT_WINDOW_HEIGHT)/2);
  glutCreateWindow("Preview3D");
  glutCreateMenu(NULL);

  // Set GLUT callbacks
  glutDisplayFunc(Display);
  glutReshapeFunc(Reshape);
  atexit(Terminate);  // Called after glutMainLoop ends
  
  // Set GLUT event callbacks
  // - Directly redirect GLUT mouse button events to AntTweakBar
  glutMouseFunc((GLUTmousebuttonfun)TwEventMouseButtonGLUT);
  // - Directly redirect GLUT mouse motion events to AntTweakBar
  glutMotionFunc((GLUTmousemotionfun)TwEventMouseMotionGLUT);
  // - Directly redirect GLUT mouse "passive" motion events to AntTweakBar (same as MouseMotion)
  glutPassiveMotionFunc((GLUTmousemotionfun)TwEventMouseMotionGLUT);
  // - Directly redirect GLUT key events to AntTweakBar
  glutKeyboardFunc((GLUTkeyboardfun)TwEventKeyboardGLUT);
  // - Directly redirect GLUT special key events to AntTweakBar
  glutSpecialFunc((GLUTspecialfun)TwEventSpecialGLUT);
  // - Send 'glutGetModifers' function pointer to AntTweakBar;
  //   required because the GLUT key event functions do not report key modifiers states.
  TwGLUTModifiersFunc(glutGetModifiers);

  // Create a tweak bar
  bar = TwNewBar("TweakBar");
  TwDefine(" GLOBAL help='This is a simple 3D mesh previewer.' "); // Message added to the help bar.
  TwDefine(" TweakBar size='200 560' color='76 76 127' "); // change default tweak bar size and color
  
  TwAddVarRO(bar,"NumberOfVertices",TW_TYPE_UINT32, &number_of_vertices,
      " label='Number of vertices' help='Displays number of vertices in mesh.'"
      " group='Mesh statistics'");
  TwAddVarRO(bar,"NumberOfFaces",TW_TYPE_UINT32, &number_of_faces,
      " label='Number of faces' help='Displays number of facs in mesh.'"
      " group='Mesh statistics'");
  // Add 'g_Zoom' to 'bar': this is a modifable (RW) variable of type TW_TYPE_FLOAT. Its key shortcuts are [z] and [Z].
  TwAddVarRW(bar, "Zoom", TW_TYPE_FLOAT, &g_Zoom, 
             " min=0.5 max=5 step=0.1 keyIncr=z keyDecr=Z help='Scale the object (1=original size).' ");
  // Add 'g_Rotation' to 'bar': this is a variable of type TW_TYPE_QUAT4F which defines the object's orientation
  TwAddVarRW(bar, "ObjRotation", TW_TYPE_QUAT4F, &g_Rotation, 
             " label='Object rotation' open help='Change the object orientation.' ");
  // Add callback to toggle auto-rotate mode (callback functions are defined above).
  TwAddVarCB(bar, "AutoRotate", TW_TYPE_BOOLCPP, SetAutoRotateCB, GetAutoRotateCB, NULL, 
             " label='Auto-rotate' key=space help='Toggle auto-rotate mode.' ");
  // Add 'g_LightMultiplier' to 'bar': this is a variable of type TW_TYPE_FLOAT. Its key shortcuts are [+] and [-].
  TwAddVarRW(bar, "Multiplier", TW_TYPE_FLOAT, &g_LightMultiplier, 
             " label='Light booster' min=0.1 max=4 step=0.02 keyIncr='+' keyDecr='-' help='Increase/decrease the light power.' ");
  // Add 'g_LightDirection' to 'bar': this is a variable of type TW_TYPE_DIR3F which defines the light direction
  TwAddVarRW(bar, "LightDir", TW_TYPE_DIR3F, &g_LightDirection, 
             " label='Light direction' open help='Change the light direction.' ");
  TwAddVarRW(bar, "ShowLines", TW_TYPE_BOOLCPP, &show_lines,  
             " group='Draw options'"
             " label='Show lines' key=L help='Toggle wire frame.'");
  TwAddVarRW(bar, "ShowFaces", TW_TYPE_BOOLCPP, &show_faces,  
             " group='Draw options'"
             " label='Show faces' key=t help='Toggle face ng.'");
  TwAddVarRW(bar, "FlatShading", TW_TYPE_BOOLCPP, &flat_shading,  
             " group='Draw options'"
             " label='Flat shading' key=T help='Toggle per face shading or per"
             " vertex shading.' ");
  TwAddVarRW(bar, "Background color", TW_TYPE_COLOR3F,
    &background_color,
    " help='Select a background color' colormode=hls");
  TwAddVarRW(bar, "LineColor", TW_TYPE_COLOR3F,
    &line_color,
    " label='Line color' help='Select a outline color' ");
  // Add 'g_MatAmbient' to 'bar': this is a variable of type TW_TYPE_COLOR4F (4 floats color)
  // and is inserted into a group named 'Material'.
  TwAddVarRW(bar, "Ambient", TW_TYPE_COLOR4F, &g_MatAmbient, " group='Material' ");
  // Add 'g_MatDiffuse' to 'bar': this is a variable of type TW_TYPE_COLOR4F (4 floats color)
  // and is inserted into group 'Material'.
  TwAddVarRW(bar, "Diffuse", TW_TYPE_COLOR4F, &g_MatDiffuse, " group='Material' ");
  TwAddVarRW(bar, "Specular", TW_TYPE_COLOR4F, &g_MatSpecular, " group='Material' ");
  TwAddVarRW(bar,"Shininess",TW_TYPE_FLOAT,&g_MatShininess," group='Material'"
      " min=0 max=128");
  
  // Store time
  g_RotateTime = glutGet(GLUT_ELAPSED_TIME);
  // Init rotation
  SetQuaternionFromAxisAngle(axis, angle, g_Rotation);
  SetQuaternionFromAxisAngle(axis, angle, g_RotateStart);
  
  
  // Load the given mesh
  char * obj_file_name;
  if(argc > 0 && argv[1] != NULL)
  {
    obj_file_name = argv[1];
  }else
  {
    obj_file_name = "/Users/ajx/Code/C++/Preview3D/cow.obj";
  }
  read_obj(obj_file_name,vertices,faces);
  get_scale_and_shift_to_fit_mesh(vertices,faces,zoom,shift);
  compute_face_normals(vertices,faces,face_normals);
  compute_vertex_normals(vertices,faces,vertex_normals);
  
  // Call the GLUT main loop
  glutMainLoop();
  
  return 0;
}

